#include "planning_base.h"

// 构造函数实现
Point::Point(const double& p_x, const double& p_y, const double& p_theta, const double& p_R)
    : x(p_x), y(p_y), thetaP(p_theta), Rp(p_R) {}


// 绘制点
void Point::ShowPoint() {
    // 设置点的颜色
    setfillcolor(BLACK);
    // 设置点的位置和半径
    solidcircle(x, y, r);
}

// 点的移动（默认移动为最小单元不可在细分）
void Point::PointMove(const double& speed_x, const double& speed_y) {
    x += speed_x;
    y += speed_y;
}

// 点绕center旋转，turn_speed为角速度(默认旋转为最小单元不可在细分，否则需要考虑时间)
void Point::PointTurn(const Point& center, const double& turn_speed) {
    thetaP += turn_speed;
    x = Rp * cos(thetaP) + center.x;
    // 在easyX库中y轴方向朝下
    y = -Rp * sin(thetaP) + center.y;
}

// 两点间距离
double Point::DistanceTo(const Point& p) const {
    return std::hypot(x - p.x, y - p.y);
}

// 计算this->相对于传入点p的角度
double Point::ThetaTo(const Point& p) const {
    // 对象点在坐标轴上/与点p重合
    if (this->x >= p.x && this->y == p.y) {
        // x轴正轴
        return 0.0;
    } else if (this->x < p.x && this->y == p.y) {
        // x轴负轴
        return PI;
    } else if (this->x == p.x && this->y < p.y) {
        // y轴正轴
        return PI / 2.0;
    } else if (this->x == p.x && this->y > p.y) {
        // y轴负轴
        return -PI / 2.0;
    } else if (this->x > p.x) {
        // 点在一、四象限内
        return -atan((this->y - p.y) / (this->x - p.x)); //考虑easyx库左y轴向下去负
    } else if (this->x < p.x) {
        // 点在二、三象限内
        return PI - -atan((this->y - p.y) / (this->x - p.x));
    }
    return 0.0;
}

// 向量的有参构造函数
Vec2d::Vec2d(const double& new_x, const double& new_y, const bool& flag)
    : x(new_x), y(new_y) {}

Vec2d::Vec2d(const Point& p_start, const Point& p_end)
    : x(p_end.x - p_start.x), y(p_start.y - p_end.y) {}

Vec2d::Vec2d(const double& length, const double& angle)
    : x(length * cos(angle)), y(length * sin(angle)) {}

// 向量的模
double Vec2d::Length() {
    return std::hypot(x, y);
}

// 向量叉乘
double Vec2d::CrossProd(const Vec2d& other) const {
    return ((this->x * other.y) - (other.x * this->y));
}

// 向量点乘
double Vec2d::InnerProd(const Vec2d& other) const {
    return ((this->x * other.x) + (this->y * other.y));
}



// 全局函数
void Delay(const double& time) {
    clock_t now = clock();
    while (clock() - now < time) {
        
    }
}

double NormalizeAngle(const double& theta) {
    // std::fmod 函数返回的是两个数相除的余数。
    double new_theta = std::fmod(theta, 2.0 * PI);
    if (new_theta > PI ) {
        new_theta -= 2.0 * PI;
    } else if (new_theta < -PI) {
        new_theta += 2.0 * PI; 
    }
    return new_theta;
}

// 角度误差修正
void CorrectAngleError(double& target_angle, const double& delta_theta) {
    if (delta_theta > 1e-6) {
        if (target_angle > 1e-6) {
            // 逆时针
            target_angle -= delta_theta;
        } else if (target_angle < -1e-6) {
            // 顺时针
            target_angle += delta_theta;
        }
    }
}

// 点到直线的距离,如果是线段就需要考虑点与线段的位置关系了
double DisPointToLine(const Point& p, const Point& p_start, const Point& p_end) {
    Vec2d line(p_start, p_end);
    Vec2d line_p(p_start, p);
    if (line.Length() == 0.0) {
        return line_p.Length();
    } else {
        return (fabs(line.CrossProd(line_p) / line.Length()));
    }
}


